#include <iostream>
#include "vision_tracker.h"
#include <cv_bridge/cv_bridge.h>
#include <pcl_conversions/pcl_conversions.h>
#include "sensor_msgs/PointCloud2.h"

VisionTracker::VisionTracker(ros::NodeHandle& n)
{
    nh = n;
    camera_info_sub = nh.subscribe("/camera/rgb/camera_info",5,&VisionTracker::camera_info_callback,this);
    image_sub = nh.subscribe("/camera/rgb/image_rect_color",5,&VisionTracker::image_callback,this);
    image_pub = nh.advertise<sensor_msgs::Image>("/vision_tracker/visualize",5);
    lane_point_pub = nh.advertise<sensor_msgs::PointCloud2>("cloud/lane",5);
    camera_matrix = Eigen::Matrix3d::Zero();

    tf::TransformListener listener;
    tf::StampedTransform transform;
    try{
        listener.waitForTransform("/base_link","/camera_link",ros::Time(0),ros::Duration(5.0));
        listener.lookupTransform("/base_link","/camera_link",ros::Time(0),transform);
    }
    catch (tf::TransformException& ex){
        ROS_ERROR("%s",ex.what());
    }

    double x = transform.getOrigin().x();
    double y = transform.getOrigin().y();
    double z = transform.getOrigin().z();

    Eigen::Matrix4d base2camera = Eigen::Matrix4d::Identity();
    base2camera(0,3) = x;
    base2camera(1,3) = y;
    base2camera(2,3) = z;

    Eigen::AngleAxisd rotation_vector ( -M_PI/2, Eigen::Vector3d ( 0,0,1 )); //角+轴：沿 Z 轴旋转 45 度
    base2camera.block<3,3>(0,0) = rotation_vector.matrix();
    
    set_camera_info = false;
    extract_lane_ptr_.reset(new ExtractLane(n));
    extract_lane_ptr_->set_transform(base2camera);
}


void VisionTracker::camera_info_callback(const sensor_msgs::CameraInfoConstPtr& msg)
{
    if(!set_camera_info){
        set_camera_info = true;
        for(int i=0;i<msg->K.size();i++)
        {
            camera_matrix(i/3,i%3) = msg->K[i];
        }
        extract_lane_ptr_->set_camera_matrix(camera_matrix);
    }
}

void VisionTracker::publish_cloud(CloudPtr& cloudptr)
{
    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(*cloudptr,cloud_msg);
    // for(int i=0;i<cloudptr->points.size();i++)
    // {
    //     std::cout<<cloudptr->points[i].x<<" "<<cloudptr->points[i].y<<" "<<cloudptr->points[i].z<<std::endl;
    // }
    cloud_msg.header.frame_id = "/base_link";
    cloud_msg.header.stamp = ros::Time::now();
    lane_point_pub.publish(cloud_msg);
}

void VisionTracker::image_callback(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try{
        cv_ptr = cv_bridge::toCvCopy(msg,"bgr8");
    }
    catch(cv_bridge::Exception& e){
        ROS_ERROR("cv bridge exception: %s",e.what());
    }
    extract_lane_ptr_->process(cv_ptr->image);
    // auto lane_cloud = extract_lane_ptr_->get_lane_cloud();
    // if(lane_cloud != NULL)
    //     publish_cloud(lane_cloud);
    // sensor_msgs::ImagePtr vis_msg = cv_bridge::CvImage(std_msgs::Header(),"bgr8",cv_ptr->image).toImageMsg();
    // image_pub.publish(vis_msg);
}


